.TITLE SUBR2

;DEFINED GLOBALS

.GLOBL PACNME,PTRTRN,PTRANS,HTRANS,PSTEP,MODTRN,EVAL,TIMER

;REQUIRED GLOBALS

.GLOBL SPEEDS,NSPEED,INBUF,OUTBUF,INSTR,LINOUT,EANGLE,UNEUL
.GLOBL GETANG,GETDIS,CLRCMA,TYPERR,FINAL,ARMS
.GLOBL SYMNME,TYPSTR,TRNPTR,EULER,PRTDIS,PRTCMA,PRTANG
.GLOBL PRTINT,FUNARG,STRING,PRTTAB,EXTIME


;REGISTER DEFINITIONS

R0	=%0
R1	=%1
R2	=%2
R3	=%3
R4	=%4
R5	=%5
SG	=%5
SP	=%6
PC	=%7
;"PACNME" - SUBR. TO PACK A SYMBOLIC NAME INTO A OUTPUT BUFFER

;THE SYMBOL DATA BLOCK ADDRESS FOR THE SYMBOL TO BE PACKED 
;MUST BE LOADED INTO R0.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#SYMBLK,R0
;		JSR	PC,PACNME
;
;NO ERROR MESSAGE IS RETURNED BY THIS ROUTINE.

;REGISTERS USED:
;
;	R0 PASSES ARGUMENT AND IS NOT MODIFIED
;	R1,SG ARE GARBAGED

PACNME:	MOV	R0,-(SP)
	ADD	#SYMNME,R0	;GET ADDRESS OF CHARACTERS
	MOV	#6,R1		;SIX CHARACTERS
PACNM1:	MOVB	(R0)+,(SG)+	;PACK AWAY THAT NAME
	SOB	R1,PACNM1
       	MOVB	#40,(SG)+	;PUT IN A SPACE AND NULL CHARACTER
	CLRB	(SG)
	MOV	(SP)+,R0
	RTS	PC

;END OF "PACNME"
;"PTRTRN" - SUBR. TO PRINT A TRANSFORMS NAME AND DATA

;THE TRANS' SYMBOL DATA BLOCK ADDRESS MUST BE LOADED INTO R0.  A 
;SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#TRNSYM,R0	;LOAD TRANSFORM ADDRESS
;		MOV	#TFFLAG,R1	;1 IF "TF" LISTING,ELSE 0
;		JSR	PC,PTRTRN
;
;AFTER EXECUTION OF PTRTRN, THE COMPUTED EULER ANGLES ARE LEFT IN
;THE ARRAY "EANGLE".  THERE IS NO ERROR MESSAGE RETURNED.

;REGISTERS USED:
;
;	R0,R1  PASS ARGUMENTS AND R1 IS MODIFIED
;	SG ARE GARBAGED

PTRTRN:	MOV	R0,-(SP)
	MOV	#OUTBUF,SG	;PACK THE TRANS NAME IN HERE
	MOV	R1,-(SP)
	BEQ	NOTTF		;TF LISTING?
	MOV	#43124,(SG)+	;YES, PACK "TF"
	MOVB	#40,(SG)+
NOTTF:	JSR	PC,PACNME
	TST	(SP)+		;NEED A COMMA IF "TF"
	BEQ	NOTTF2
	MOVB	#54,(SG)+
	CLRB	(SG)
NOTTF2:	MOV	#OUTBUF,SG	;TYPE THE NAME
	JSR	PC,TYPSTR
	MOV	TRNPTR(R0),R0	;GET PTR TO TRANS DATA
	BNE	GOTDAT
	MOV	#PTRMES,SG	;SAY NOT DEFINED IF NO DATA
	JSR	PC,LINOUT
	BR	.+6
GOTDAT:	JSR	PC,PTRANS	;PRINT X,Y,Z,O,A,T
	MOV	(SP)+,R0
	RTS	PC

PTRMES:	.ASCII	/TRANSFORMATION DATA NOT YET DEFINED/
	.BYTE	0
	.EVEN

;END OF "PTRTRN"
;"PTRANS" - SUBR. TO PRINT A TRANSFORMS X,Y,Z,O,A,T

;THE TRANS DATA ADDRESS MUST BE LOADED INTO R0.  A SAMPLE CALLING 
;SEQUENCE FOLLOWS:
;
;		MOV	#TRANS,R0	;LOAD TRANSFORM ADDRESS
;		JSR	PC,PTRANS
;
;AFTER EXECUTION OF PTRANS, THE COMPUTED EULER ANGLES ARE LEFT IN
;THE ARRAY "EANGLE".  THERE IS NO ERROR MESSAGE RETURNED.

;REGISTERS USED:
;
;	R0 PASSES ARGUMENT AND IS NOT MODIFIED
;	R1,SG ARE GARBAGED

PTRANS:	MOV	R0,-(SP)	;SAVE TRANSFORM POINTER
	MOV	R2,-(SP)
	MOV	R3,-(SP)
	MOV	#EANGLE,R1	;CONVERT TRANS TO EULER ANGLES
	JSR	PC,EULER
	MOV	#OUTBUF,SG	;POINT TO START OF OUTPUT STRING
	MOV	#EANGLE,R2
       	MOV	#3,R3		;SET LOOP COUNT TO OUTPUT X,Y,Z
PTRAN1:	MOV	(R2)+,R0 	;CONVERT DISTANCE TO ASC
	JSR	PC,PRTDIS
	JSR	PC,PRTCMA
	SOB	R3,PTRAN1	
       	MOV	#3,R3		;SET LOOP COUNT TO OUTPUT O,A,T
PTRAN2:	MOV	(R2)+,R0 	;CONVERT ANGLES TO ASC
	JSR	PC,PRTANG
	JSR	PC,PRTCMA
	SOB	R3,PTRAN2
	SUB	#2,SG		;PUT IN A NULL CHARACTER
	CLRB	(SG)
	MOV	#OUTBUF,SG	;OUTPUT THE STRING
	JSR	PC,LINOUT
	MOV	(SP)+,R3	;RESTORE REGISTERS
	MOV	(SP)+,R2
       	MOV	(SP)+,R0
	RTS	PC

HTRANS:	.ASCII	/          X        Y        Z         O/
	.ASCII	/        A        T/
	.BYTE	0
	.EVEN

;END OF "PTRANS"
;"PSTEP"  - SUBR. TO PRINT MOTION INSTRUCTION OUT ON TTY

;A POINTER TO THE DATA BLOCK CONTAINING THE MOTION INSTRUCTION MUST
;BE LOADED INTO R1 AND THE STEP NUMBER MUST BE SET IN R0.  IF THE
;DATA BLOCK POINTER IS NON-ZERO, THE MOTION INSTRUCTION IS CONVERTED
;TO ASC ALONG WITH ITS STEP NUMBER AND THEY ARE TYPED OUT.
;OTHERWISE, NO TYPE OUT OCCURS.  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#STEPNUM,R0
;		MOV	#BLKPTR,R1
;		JSR	PC,PSTEP
;
;AT THE END OF EXECUTION, "OUTBUF" IS ALWAYS LEFT WITH AT LEAST
;THE STEP NUMBER CODED IN ASC.  THERE IS NO ERROR  MESSAGE
;RETURNED FROM THIS ROUTINE.

;REGISTERS USED:
;
;	R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
;	SG IS GARBAGED

PSTEP:	MOV	R4,-(SP)
	MOV	R3,-(SP)
	MOV	R2,-(SP)
	MOV	R1,-(SP)	;SAVE STEP POINTER
	MOV	#OUTBUF,SG	;CONSTRUCT ASC STRING IN HERE
	JSR	PC,PRTINT	;STEP NUMBER
	MOVB	#40,(SG)+	;SPACE CHARACTER
	MOV	(SP),R4		;ALL DONE IF NO INSTRUCTION
	BEQ	PSTDNE
	TST	(R4)+
	MOV	(R4)+,R0	;MOTION FUNCTION SYMBOL BLOCK
	JSR	PC,PACNME	;NAME TO ASC
	MOV	FUNARG+2(R0),R2	;SPECIFICATIONS OF ARGUMENTS
	MOV	FUNARG(R0),R3
	BEQ	PSPTYP		;GO TYPE NAME IF NO ARGS
       	CMP	#STRING,R3	;SPECIAL CASE OF 1 STRING ARG
	BNE	PACARG
       	MOVB	(R4)+,(SG)+	;PACK AWAY STRING ARGUMENT
	BNE	.-2
	BR	PSPTYP
PRTARG:	BIC	#170000,R2	;DONT WANT SIGN BIT EXTENDED
PACARG:	MOV	R3,R1		;NEXT ARGUMENT TYPE
	BIC	#177761,R1
	MOV	(R4)+,R0	;NEXT ARGUMENT
	JSR	PC,@PRTTAB(R1)	;CONVERT TO ASC
	JSR	PC,PRTCMA	;COMMA
	ASHC	#-4,R2		;REPEAT FOR ALL ARGUMENTS
	BNE	PRTARG
	CLRB	-2(SG)
PSPTYP:	MOV	#OUTBUF,SG	;TYPE THE MOTION COMMAND
	JSR	PC,LINOUT
PSTDNE:	MOV	(SP)+,R1
	MOV	(SP)+,R2
	MOV	(SP)+,R3
	MOV	(SP)+,R4
	RTS	PC

;END OF "PSTEP"
;"MODTRN" - SUBR. TO PERMIT MODIFICATION OF EXISTING TRANSFORMS

;THIS SUBROUTINE IS CALLED TO ALLOW THE USER TO EDIT EXISTING
;TRANSFORMS.  THE ONLY REQUIRED ARGUMENT TO THIS ROUTINE IS A TRANS
;POINTER LOADED INTO REGISTER R0.  EDITING IS CONTINUED INDEFINITLY
;UNTIL THE USER RESPONSES TO THE QUERY "CHANGES" WITH A NULL LINE 
;(I.E. NO REQUESTED CHANGES ).  A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;		MOV	#TRANS,R0	
;		JSR	PC,MODTRN
;
;THERE IS NO ERROR RETURN FROM THIS ROUTINE

;REGISTERS USED:
;
;	R0 PASSES ARGUMENT AND IS NOT MODIFIED
;	R1,R2,R3,R4,SG ARE GARBAGED

MODTRN:	MOV	R0,-(SP)
       	MOV     #HTRANS+7,SG	;TYPE OUT THE COLUMN HEADER
	JSR	PC,LINOUT
	BR	MODT1
CHGTRN:	MOV	#EANGLE,R1	;CONVERT EULER ANGLES BACK TO TRANS
	MOV	(SP),R0
	JSR	PC,UNEUL
MODT1:	MOV	(SP),R0
       	JSR	PC,PTRANS	;TYPE OUT THIS TRANSFORM
	MOV     #CHGMES,SG	;ASK FOR CHANGES
	JSR	PC,LINOUT
	MOV	#INBUF,SG	;READ IN THE CHANGES
	JSR	PC,INSTR
	MOV	#EANGLE,R4	;EULER ANGLES ARE STORED IN HERE
	CLR	-(SP) 		;KEEP TRACK OF ANY CHANGES
	MOV	#GETDIS,R2	;READ IN THE THREE DISTANCES
MODT2:	MOV	#3,R3		;SET LOOP COUNTER
MODT3:	JSR	PC,(R2)
	BCC	ISCORR		;BRANCH IF A CORRECTION WAS TYPED IN
	TST	R1		;BRANCH IF ERROR ON INPUT
	BNE	MODERR
	TST	(R4)+		;NO CHANGE MADE
	BR	NOCORR
ISCORR:	MOV	R0,(R4)+	;CHANGE EULER ANGLE
	INC     (SP)  		;INDICATE CHANGE MADE
NOCORR:	JSR	PC,CLRCMA	;SKIP OVER COMMA
	BCC	MODCOM		;BRANCH IF NO ERROR
MODERR:	JSR	PC,TYPERR	;TYPE INPUT ERROR MESSAGE
	TST	(SP)+
	BR	MODT1
MODCOM:	SOB	R3,MODT3	;REPEAT FOR ALL NUMBERS
	CMP	#GETANG,R2	;REPEAT FOR 3 ANGLES
	BEQ	MODT4
	MOV	#GETANG,R2
	BR	MODT2
MODT4:	TST	(SP)+      	;REPEAT IF CORRECTIONS MADE
	BNE	CHGTRN
	MOV	(SP)+,R0
       	RTS	PC			

CHGMES:	.ASCII	/CHANGE?/
	.BYTE	0
	.EVEN

;END OF "MODTRN"
;"EVAL"   - EVALUATES A 5TH ORDER POLYNOMIAL 

;"EVAL" COMPUTES THE DESIRED PERCENTAGE CHANGE IN SET POINTS BASED
;UPON THE FRACTION OF TIME THAT HAS ELAPSED SINCE THE START OF A
;MOTION.  IF "PTIME" IS THE TIME FOR WHICH THE SET POINT EVALUATION
;IS DESIRED AND "TTIME" IS THE TOTAL MOTION TIME, THE DESIRED
;PERCENTAGE CHANGE IN SET POINT WILL BE:
;
;		% CHANGE = 6.0*T↑5 -15*T↑4 +6*T↑3 -1
;  WHERE       	       T = PTIME/TTIME
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
;			MOV	PTIME,R0
;			MOV	#JTARAY,R1
;			MOV	TTIME,R2
;			JSR	PC,EVAL
;
;THE PERCENTAGE CHANGE IS RETURNED IN R0 WHERE 1.0 = '40000. IF PTIME
;IS GREATER THAN OR EQUAL TO TTIME, R0 IS SET TO ZERO AND THE 
;"FINAL" FLAG BIT IS SET IN THE 8TH WORD OF THE "JTARAY" ARRAY.

;REGISTERS USED:
;	R0,R2 PASS ARGUMENTS AND ARE ALTERED
;	R1,R3 ARE GARBAGED

EVAL:	CMP	R2,R0		;PAST END OF TRAJECTORY?
	BLE	TRJEND		;YES
	CLR	R1		;% TIME = (PTIME/TTIME)
	ASHC	#-1,R0
	DIV	R2,R0
	TST	R1		;ROUND
	BPL	.+4
	INC	R0
	MOV	#30000,R2	;6.0 x T
	MUL	R0,R2
	ASHC	#1,R2
	TST	R3
	BPL	.+4
	INC	R2
	SUB	#74000,R2	?- 15.0
	MUL	R0,R2		;x T
	ASHC	#1,R2
	TST	R3
	BPL	.+4
	INC	R2
	ADD	#50000,R2	;+ 10.0
	MOV	#3,R1		;x T**3
TCUBE:	MUL	R0,R2
	ASHC	#2,R2
	TST	R3
	BPL	.+4
	INC	R2
	SOB	R1,TCUBE
	MOV	R2,R0
	SUB	#40000,R0	;-1.0
	BR	EVALDN

TRJEND:	CLR	R0		;USE FINAL SET POINT
	BIS	#FINAL,16(R1)	;SET POINT EVALUATION DONE

EVALDN:	RTS	PC

;END OF "EVAL"
;"TIMER"  - COMPUTE"TOTAL MOTION TIME

;DETERMINES THE TOTAL TIME REQUIRED FOR AN ARM MOTION BY COMPUTING
;THE INDIVIDUAL TIMES REQUIRED BY EACH JOINT AND TAKING THE LARGEST.
;A SAMPLE CALLING SEQUENCE TO THIS ROUTINE FOLLOWS:
;
;			MOV	#CHANGE,R0 
;			JSR	PC,TIMER
;			MOV	R0,TIME
;
;THE ONLY ARGUMENT TO THIS ROUTINE IS A POINTER TO A TABLE CONTAINING
;THE CHANGE IN THE JOINT ANGLES FOR THE DESIRED MOTION.

;REGISTERS USED:
;	R0 PASSES ARGUMENTS AND IS ALTERED
;	R1,R2,R3,R4 ARE GARBAGED

TIMER:	MOV	R5,-(SP)
	MOV	R0,R5
	MOV	#SPEEDS,R1	;TABLE OF MAXIMUM JOINT SPEEDS
	MOV	#6,R4		;SIX JOINTS TO TIME
	CLR	R0		;MAXIMUM TRAVERSE TIME
SPDLP:	MOV	(R5)+,R2	;COMPUTE JT TRAVERSE TIME
	BGE	.+4
	NEG	R2
	MUL	(R1)+,R2
	TST	R3		;ROUND UP
	BPL	.+4
	INC	R2
	CMP	R2,R0		;KEEP MAXIMUM TIME
	BLE	.+4
	MOV	R2,R0
	SOB	R4,SPDLP
	TST	R0		;TIME = 0?
	BEQ	ZEROT
	ADD	@#EXTIME,R0	;ADD A LITTLE TIME FOR SHORT MOVES
	BVC	.+6
	MOV	#77777,R0	;SET TO MAX IF OVERFLOW
ZEROT:	TST	@#NSPEED	;USER REQUESTED CHANGED?
	BEQ	TMEDNE		;NO
	MUL	@#NSPEED,R0	;YES, CORRECT
	CLR	@#NSPEED	;ONLY USE ONCE
	ASHC	#-9.,R0		;NORMALIZE
	TST	R0		;SET TO MAX IF OVERFLOW
	BNE	MAXTME
	MOV	R1,R0
	BPL	.+6
MAXTME:	MOV	#77777,R0	;MAXIMUM PERMITTED TIME
TMEDNE:	MOV	(SP)+,R5
	RTS	PC

.END